#include "cv_lidar.hpp"

#include "gtest/gtest.h"
#include "lidar.hpp"
#include "log.hpp"
#include "opencv2/opencv.hpp"

// TEST(TestCV, TestLidar) {
//   auto image = component::ViualizeBackground();
//   cv::imshow("background", image);
//   cv::waitKey(0);
//   cv::imwrite("../../../image/background.png", image);
// }

// TEST(TestCV, TestUnique) {
//   std::vector<cv::Point2d> vec{
//       cv::Point2d(50, 50), cv::Point2d(50, 50), cv::Point2d(50, 50),
//       cv::Point2d(50, 50), cv::Point2d(50, 60), cv::Point2d(60, 50),
//       cv::Point2d(40, 50), cv::Point2d(50, 60), cv::Point2d(60, 50),
//       cv::Point2d(40, 40), cv::Point2d(50, 60), cv::Point2d(40, 50),
//   };
//   std::sort(vec.begin(), vec.end(), [](cv::Point2d a, cv::Point2d b) {
//     return (a.x == b.x) ? a.y < b.y : a.x < b.x;
//   });
//   vec.erase(std::unique(vec.begin(), vec.end()), vec.end());
//   std::cout << vec << std::endl;
// }

// TEST(TestCV, TestBackground) {
//   // auto image = cv::Mat(500, 500, cv::8UC3, component::kBLACK);
//   cv::Mat image = cv::Mat(400, 400, CV_8UC3, painting::kBLACK);
//   cv::circle(image, {200, 200}, 150, painting::kYELLOW, 1);
//   cv::circle(image, {200, 200}, 50, painting::kRED, 1);
//   cv::Point o = cv::Point(200, 200), t;
//   for (int i = 0; i < 6; i++) {
//     float theta = (180 / 6 * i - 180) * 1.0 / 180.0 * M_PI;
//     SPDLOG_INFO("{}, {}", i, theta);
//     t = cv::Point(150 * sin(theta), 150 * cos(theta));
//     cv::line(image, o + t, o - t, painting::kYELLOW, 1);
//   }
//   cv::imshow("background", image);
//   cv::waitKey(0);
//   cv::imwrite("../../../image/background2.png", image);
// }

TEST(TestCV, TestLidar) {
  component::Logger::SetLogger("logs/lidar_cv.log");
  cv::Mat frame = cv::imread("../../../image/background2.png");
  LidarDriver lidar;
  lidar.Init();
  // while (true) {
  for (int i = 1; i <= 5000; i++) {
    auto px = lidar.GetInner();
    auto p = lidar.Apply();
    auto f = VisualizeMeasure(frame.clone(), p);
    auto f2 = VisualInner(frame.clone(), px);

    cv::imshow("show", f);
    cv::imshow("show2", f2);
    if (cv::waitKey(2) == ' ') exit(0);
  }
}